import csv
import os
import tf
import matplotlib.pyplot as plt

directory_drive = "<<path to the directory containing the data>>"

food_list = ["<directory containing the food item1>", "<directory containing the food item2>", "<directory containing the food item14>"]

def plotValues(title, x_values, 
                y1_values, y2_values, y3_values,
                y4_values, y5_values, y6_values,
                y7_values, y8_values, y9_values,
                y10_values, y11_values, y12_values, 
                y13_values, y14_values, y15_values,
                y16_values, y17_values, y18_values):
    
    plt.title(title)
    plt.grid(True)
    plt.ylabel('Force (N)')
    plt.xlabel('Time (s)')
    plt.plot(x_values, y1_values, label='force_x')
    plt.plot(x_values, y2_values, label='force_y')
    plt.plot(x_values, y3_values, label='force_z')
    plt.legend(loc='upper right')

def plotFiles(subject_num, food_num, trial_num):

    time_list = []
    x_force_list = []
    y_force_list = []
    z_force_list = []
    x_torque_list = []
    y_torque_list = []
    z_torque_list = []
    x_pos_list = []
    y_pos_list = []
    z_pos_list = []
    x_ori_list = []
    y_ori_list = []
    z_ori_list = []
    x_vel_pos_list = []
    y_vel_pos_list = []
    z_vel_pos_list = []
    x_vel_ori_list = []
    y_vel_ori_list = []
    z_vel_ori_list = []

    sub_directory = "subject"+str(subject_num)+"/"+food_list[food_num]+str(trial_num)+".csv"
    file_directory = directory_drive+sub_directory
    data_file = open(file_directory,'r')
    csv_data = csv.reader(data_file, delimiter=',')
    
    csv_data.next()

    for row in csv_data:
        time_list.append(row[0])
        x_force_list.append(float(row[1]))
        y_force_list.append(float(row[2]))
        z_force_list.append(float(row[3]))
        x_torque_list.append(float(row[4]))
        y_torque_list.append(float(row[5]))
        z_torque_list.append(float(row[6]))
        x_pos_list.append(float(row[7]))
        y_pos_list.append(float(row[8]))
        z_pos_list.append(float(row[9]))
        x_ori_list.append(float(row[10]))
        y_ori_list.append(float(row[11]))
        z_ori_list.append(float(row[12]))
        x_vel_pos_list.append(float(row[13]))
        y_vel_pos_list.append(float(row[14]))
        z_vel_pos_list.append(float(row[15]))
        x_vel_ori_list.append(float(row[16]))
        y_vel_ori_list.append(float(row[17]))
        z_vel_ori_list.append(float(row[18]))
    
    data_file.close()
    
    plotValues(sub_directory, time_list, 
    x_force_list, y_force_list, z_force_list,
    x_torque_list, y_torque_list, z_torque_list, 
    x_pos_list, y_pos_list, z_pos_list,
    x_ori_list, y_ori_list, z_ori_list, 
    x_vel_pos_list, y_vel_pos_list, z_vel_pos_list,
    x_vel_ori_list, y_vel_ori_list, z_vel_ori_list)

subject_num = 1 # subject_num ranges from 1 to 12.
food_num = 0 # food_num ranges from 0 to 13 to indicate one of the food in food_list array. 
trial_num = 1 # trial_num ranges from 1 to 20 for all subjects except for the subject 10. Subject 10 has 16 trials for each food.

for s_num in range(subject_num,subject_num+1):
    for f_num in range(food_num,food_num+1):
        for t_num in range(trial_num, trial_num+1):
            plotFiles(s_num, f_num, t_num)

plt.show()
